#include "imu.h"
#include "bmi088.h"

#include "element.h"
#include "pid.h"
#include "MahonyAHRS.h"
#include "mymath.h"
#include "dma.h"
#include "zf_delay.h"
#include "zf_pwm.h"
#include "zf_gpio.h"
#include "zf_eeprom.h"
#include <string.h>

IMU_type IMU;

// 陀螺仪温控闭环PID
static pid_type_def IMU_Temp_Loop;
const static fp32 IMU_Temp_Loop_PID[3] = {743.82, 23.13, 3763.91};
// 读取E2PROM缓存
static uint8 xdata tempBuff[24];
// 加速度计低通滤波
static fp32 accel_fliter_1[3] = {0.0f, 0.0f, 0.0f};
static fp32 accel_fliter_2[3] = {0.0f, 0.0f, 0.0f};
static const fp32 fliter_num[3] = {1.929454039488895f, -0.93178349823448126f, 0.002329458745586203f};

void IMU_Init(void)
{
    uint8 i;
    pwm_init(PWMB_CH3_P76, 50, 0);
    // gpio_mode(P7_6, GPO_PP);
    // IMU_TEMP_Pin = DISABLE;

    IMU.CaliGyro = DISABLE;
    IMU.CaliZero = DISABLE;
    
    // 重置角度
    IMU_Reset();

    // 陀螺仪温控闭环
    PID_init(&IMU_Temp_Loop, PID_POSITION, IMU_Temp_Loop_PID, 10000, 6000);

    // 打开预热开关
    IMU.Preheat = ENABLE;

    // 读取e2prom
    iap_read_bytes(CALI_GYRO_DATA_ADDR, tempBuff, 24);
    // 把缓存的值读取出来
    for (i = 0; i < 3; i++)
    {
        memcpy(&IMU.GyroOffset[i], &tempBuff[i * 4], 4);
        memcpy(&IMU.AngleOffset[i], &tempBuff[i * 4 + 12], 4);
    }
}

/**
 * @brief          IMU加热
 * @param[in]      none
 * @param[out]     none
 * @retval         none
 */
void IMU_Heating(void)
{
    static uint8 s_wCount;
    s_wCount++;
    if (s_wCount > 100)
    {
        s_wCount = 0;
        BMI088_GetTemperate(&IMU.Temp);
        PID_calc(&IMU_Temp_Loop, IMU.Temp, 50);
        pwm_duty(PWMB_CH3_P76, LIMIT_P(IMU_Temp_Loop.out, 10000));
    }
}

/**
 * @brief          IMU开机预热
 * @param[in]      none
 * @param[out]     none
 * @retval         none
 */
void IMU_Preheat(void)
{
    delay_ms(2);
    // IMU加热
    IMU_Heating();
    // 时间到会开始判断
    if (Counter.Main >= 100)
    {
        Counter.Main = 0;
        if (IMU.Temp >= 49)
        {
            IMU.Preheat = DISABLE;
        }
    }
}

void IMU_Reset(void)
{
    IMU.Quat[0] = 1.0f;
    IMU.Quat[1] = 0.0f;
    IMU.Quat[2] = 0.0f;
    IMU.Quat[3] = 0.0f;
    IMU.EulerAngle[0] = 0.0f;
    IMU.EulerAngle[1] = 0.0f;
    IMU.EulerAngle[2] = 0.0f;
}

void IMU_Process(void)
{
    // 获取陀螺仪数据
    BMI088_read(IMU.RawGyro, IMU.RawAccel);

    // 陀螺仪去零飘
    IMU.RawGyro[x] += IMU.GyroOffset[x];
    IMU.RawGyro[y] += IMU.GyroOffset[y];
    IMU.RawGyro[z] += IMU.GyroOffset[z];
    // 测到过还不错的校准值
    //  IMU.RawGyro[x] += -0.006099;
    //  IMU.RawGyro[y] += 0.004444;
    //  IMU.RawGyro[z] += -0.002793;

    // 加速度计FIR滤波
    accel_fliter_1[0] = accel_fliter_2[0];
    accel_fliter_2[0] = IMU.FiltAccel[0];
    IMU.FiltAccel[0] = accel_fliter_2[0] * fliter_num[0] + accel_fliter_1[0] * fliter_num[1] + IMU.RawAccel[x] * fliter_num[2];

    accel_fliter_1[1] = accel_fliter_2[1];
    accel_fliter_2[1] = IMU.FiltAccel[1];
    IMU.FiltAccel[1] = accel_fliter_2[1] * fliter_num[0] + accel_fliter_1[1] * fliter_num[1] + IMU.RawAccel[y] * fliter_num[2];

    accel_fliter_1[2] = accel_fliter_2[2];
    accel_fliter_2[2] = IMU.FiltAccel[2];
    IMU.FiltAccel[2] = accel_fliter_2[2] * fliter_num[0] + accel_fliter_1[2] * fliter_num[1] + IMU.RawAccel[z] * fliter_num[2];

    // 陀螺仪数据融合，计算四元数
    // MahonyAHRSupdateIMU(IMU.Quat, IMU.RawGyro[x], IMU.RawGyro[y], IMU.RawGyro[z], IMU.RawAccel[x], IMU.RawAccel[y], IMU.RawAccel[z]);
    MahonyAHRSupdateIMU(IMU.Quat, IMU.RawGyro[x], IMU.RawGyro[y], IMU.RawGyro[z], IMU.FiltAccel[x], IMU.FiltAccel[y], IMU.FiltAccel[z]);
    // 四元数转欧拉角
    Quat_CalcAngle(IMU.Quat, &IMU.EulerAngle[yaw], &IMU.EulerAngle[pitch], &IMU.EulerAngle[roll]);
    // 让欧拉角归零
    //  IMU.EulerAngle[yaw]+=IMU.AngleOffset[yaw];//yaw不需要矫正
    IMU.EulerAngle[pitch] += IMU.AngleOffset[pitch];
    IMU.EulerAngle[roll] += IMU.AngleOffset[roll];
    // IMU定时加热
    IMU_Heating();
}

/**
 * @brief          设定偏航角Yaw
 * @param[in]      _Angle:要偏转的角度
 * @param[out]     none
 * @retval         none
 */
void IMU_SetYaw(fp32 _Angle)
{
    // 加上要设定的角度，变成方位
    IMU.SetYaw = IMU.EulerAngle[yaw] + (_Angle * Elem.OutDirect);
    // 为了把数轴绕成环，>180则减去360，<-180则加上360
    IMU.SetYaw = IMU.SetYaw > 180 ? IMU.SetYaw - 360 : IMU.SetYaw;
    IMU.SetYaw = IMU.SetYaw < -180 ? IMU.SetYaw + 360 : IMU.SetYaw;
}

/**
 * @brief          计算设定的偏航角与实际偏航角的差，变成电感、编码器一样
 * @param[in]      none
 * @param[out]     要偏转的角度
 * @retval         none
 */
fp32 IMU_CalcYawRef(void)
{
    fp32 _refAngle, temp;
    // 计算环其中一个方向的反馈角度_refAngle
    _refAngle = IMU.SetYaw - IMU.EulerAngle[yaw];
    // 计算环中与上述方向相反的反馈角度temp
    // 若角度>=0，则-360；若角度<0，则+360
    if (_refAngle >= 0)
    {
        temp = _refAngle - 360;
    }
    else
    {
        temp = _refAngle + 360;
    }
    // 比较两个方向的耗费距离，选择最短的上报
    _refAngle = M_ABS(_refAngle) > M_ABS(temp) ? (temp) : _refAngle;
    return _refAngle;
}

void IMU_GyroOffsetCalc(void)
{
    static uint16 Count, i;
    static fp32 TempOffset[3];

    if (IMU.CaliGyro == ENABLE)
    {
        Count++;
        TempOffset[x] -= IMU.RawGyro[x];
        TempOffset[y] -= IMU.RawGyro[y];
        TempOffset[z] -= IMU.RawGyro[z];
        if (Count >= 10000)
        {
            Count = 0;
            IMU.CaliGyro = DISABLE;
            IMU.CaliZero = ENABLE;
            // 10000次，总共20s同RM例程给一样，效果还不错
            IMU.GyroOffset[x] = TempOffset[x] / 10000.0f;
            IMU.GyroOffset[y] = TempOffset[y] / 10000.0f;
            IMU.GyroOffset[z] = TempOffset[z] / 10000.0f;
            TempOffset[x] = 0;
            TempOffset[y] = 0;
            TempOffset[z] = 0;
        }
    }
    if (IMU.CaliZero == ENABLE)
    {
        Count++;
        TempOffset[yaw] -= IMU.EulerAngle[yaw];
        TempOffset[pitch] -= IMU.EulerAngle[pitch];
        TempOffset[roll] -= IMU.EulerAngle[roll];
        if (Count >= 2000)
        {
            Count = 0;
            IMU.CaliZero = DISABLE;
            // 2000次足够了，只是让pitch,roll归零
            IMU.AngleOffset[yaw] = TempOffset[yaw] / 2000.0f;
            IMU.AngleOffset[pitch] = TempOffset[pitch] / 2000.0f;
            IMU.AngleOffset[roll] = TempOffset[roll] / 2000.0f;
            TempOffset[yaw] = 0;
            TempOffset[pitch] = 0;
            TempOffset[roll] = 0;
            // 把值写入缓存
            for (i = 0; i < 3; i++)
            {
                memcpy(&tempBuff[i * 4], &IMU.GyroOffset[i], 4);
                memcpy(&tempBuff[i * 4 + 12], &IMU.AngleOffset[i], 4);
            }
            // 擦除原有的数据
            iap_erase_page(CALI_GYRO_DATA_ADDR + 511);
            // 保存校准数据到e2prom
            iap_write_bytes(CALI_GYRO_DATA_ADDR, tempBuff, 24);
        }
    }
}

// IMU旋转，用旋转矩阵把imu实际位置和小车位置对应上
/*
  #def ine BMI088_BOARD_INSTALL_SPIN_MATRIX    \
     {0.0f, 1.0f, 0.0f},                     \
     {-1.0f, 0.0f, 0.0f},                     \
     {0.0f, 0.0f, 1.0f}                      \


 #define IST8310_BOARD_INSTALL_SPIN_MATRIX   \
     {1.0f, 0.0f, 0.0f},                     \
     {0.0f, 1.0f, 0.0f},                     \
     {0.0f, 0.0f, 1.0f}                      \
*/

/**
 * @brief          旋转陀螺仪,加速度计和磁力计,并计算零漂,因为设备有不同安装方式
 * @param[out]     gyro: 加上零漂和旋转
 * @param[out]     accel: 加上零漂和旋转
 * @param[out]     mag: 加上零漂和旋转
 * @param[in]      bmi088: 陀螺仪和加速度计数据
 * @param[in]      ist8310: 磁力计数据
 * @retval         none
 */
// static void imu_cali_slove(fp32 gyro[3], fp32 accel[3], fp32 mag[3], bmi088_real_data_t *bmi088, ist8310_real_data_t *ist8310)
// {
//     for (uint8_t i = 0; i < 3; i++)
//     {
//         gyro[i] = bmi088->gyro[0] * gyro_scale_factor[i][0] + bmi088->gyro[1] * gyro_scale_factor[i][1] + bmi088->gyro[2] * gyro_scale_factor[i][2] + gyro_offset[i];
//         accel[i] = bmi088->accel[0] * accel_scale_factor[i][0] + bmi088->accel[1] * accel_scale_factor[i][1] + bmi088->accel[2] * accel_scale_factor[i][2] + accel_offset[i];
//         mag[i] = ist8310->mag[0] * mag_scale_factor[i][0] + ist8310->mag[1] * mag_scale_factor[i][1] + ist8310->mag[2] * mag_scale_factor[i][2] + mag_offset[i];
//     }
// }
